﻿
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using PS.DataTypes;

namespace PS.Algorithms
{
    public class Dijkstra
    {
        public static void Run(Graph graph, Vertex start, Vertex goal)
        {
            Vertex[] vertices = graph.Vertices;
            PriorityQueue<float, Vertex> openNodes = new PriorityQueue<float, Vertex>();

            // Load all vertices from the graph
            for (int i = 0; i < graph.Vertices.Length; i++)
            {
                Vertex v = vertices[i];

                // Startvertex has distance 0
                if (v == start)
                {
                    v.Distance = 0;
                }
                else
                {
                    v.Distance = float.PositiveInfinity;
                }

                v.Previous = null;

                openNodes.Enqueue(v.Distance, v);
            }

            while (!openNodes.IsEmpty)
            {
                Vertex v = openNodes.Dequeue();

                // Terminate if we reached the goal
                if (v == goal) break;

                float distance = v.Distance;
                if (distance == float.PositiveInfinity)
                {
                    // Unreachable vertices are not processed
                    continue;
                }

                // The relaxation process updates the costs of all the vertices connected to vertex v
                Vertex[] neighbours = graph.GetAdjacentVertices(v);
                foreach (Vertex n in neighbours)
                {
                    Edge e = graph.IncidentEdge(v, n);
                    float weight = e.Weight;
                    float distanceSoFar = distance + weight;
                    if (distanceSoFar < n.Distance)
                    {
                        openNodes.Update(n.Distance, distanceSoFar, n);
                        n.Distance = distanceSoFar;
                        n.Previous = v;
                    }
                }
            }
        }

        public static Vertex[] GetShortestPath(Graph g, Vertex start, Vertex goal)
        {
            Run(g, start, goal);

            Vertex[] path = new Vertex[g.Edges.Length];

            Vertex previous = goal;
            int i = 0;

            previous = goal;

            while (previous != null)
            {
                path[i++] = previous;
                previous = previous.Previous;
            }

            Vertex[] pathResized = (from p in path where p != null select p).ToArray();
            Array.Reverse(pathResized);

            return pathResized;
        }
    }
}